#include <iostream>
#include <opencv2/opencv.hpp>
#include <queue>
#include <vector>
#include <algorithm>
#include <limits>
#include <cmath>
/****************************************
 **********移动机器人规划控制入门与实践*******
 **************BFS示例代码****************
 * 包含：
 * 图片转栅格地图；
 * BFS搜索算法；
 * 基于opencv的拓展过程显示。
 ****************************************
 */

// 定义二维点
struct Point
{
  int x;
  int y;
  bool operator==(const Point &other) const { return x == other.x && y == other.y; }
};

// 定义栅格大小
const int occ_size = 11;

// 辅助函数——标记障碍物
void markObstacles(cv::Mat &image, const std::vector<std::vector<int>> &grid)
{
  for (std::vector<std::vector<int>>::size_type y = 0; y < grid.size(); ++y)
  {
    for (std::vector<std::vector<int>>::size_type x = 0; x < grid[0].size(); ++x)
    {
      if (grid[y][x] == 1)
      {
        // 使用黑色标记障碍物
        cv::Rect roi(x * occ_size, y * occ_size, occ_size, occ_size);
        image(roi) = cv::Vec3b(0, 0, 0); // 黑色
      }
    }
  }
}

// 辅助函数——标记起点
void markStart(cv::Mat &image, Point point)
{
  // 使用蓝色标记起点
  cv::Rect roi(point.x * occ_size, point.y * occ_size, occ_size, occ_size);
  image(roi) = cv::Vec3b(255, 0, 0); // Blue color
}
// 辅助函数——标记终点
void markGoal(cv::Mat &image, Point point)
{
  // 使用深红标记终点
  cv::Rect roi(point.x * occ_size, point.y * occ_size, occ_size, occ_size);
  image(roi) = cv::Vec3b(0, 0, 125); // Red color       //Blue   Green   Red
}

std::vector<Point> bfsSearch(std::vector<std::vector<int>> &grid, Point start, Point end, cv::Mat &image)
{
  int rows = grid.size();                                                           // 行数 y
  int cols = grid[0].size();                                                        // 列数 x
  std::vector<std::vector<bool>> visited(rows, std::vector<bool>(cols, false));     // 创建一个与栅格地图同样大小的二维向量，记录节点是否被访问过
  std::vector<std::vector<Point>> parent(rows, std::vector<Point>(cols, {-1, -1})); // 创建一个与栅格地图同样大小的二维向量，存储每个节点的父节点

  std::queue<Point> q;
  q.push(start);
  visited[start.y][start.x] = true;

  // 定义节点可以拓展的方向：以节点为中心，上下左右，四连通
  int dx[] = {-1, 0, 1, 0};
  int dy[] = {0, 1, 0, -1};

  while (!q.empty())
  {
    Point current = q.front();
    q.pop();

    // 判断是否到达终点
    if (current.x == end.x && current.y == end.y)
    {
      // 回溯路径
      std::vector<Point> path;
      while (current.x != -1 && current.y != -1)
      {
        path.push_back(current);
        current = parent[current.y][current.x];
      }
      std::reverse(path.begin(), path.end());
      return path;
    }

    // 节点拓展过程
    for (int i = 0; i < 4; ++i)
    {
      int nx = current.x + dx[i];
      int ny = current.y + dy[i];

      // 排除异常情况：（1）节点出界；（2）已经访问过了；（3）节点为障碍物
      if (nx >= 0 && nx < cols && ny >= 0 && ny < rows && !visited[ny][nx] && grid[ny][nx] == 0)
      {
        q.push({nx, ny});         // 加入队列
        visited[ny][nx] = true;   // 设置为已经访问过，后续就不用再加入队列
        parent[ny][nx] = current; // 设置父节点

        // 显示该节点
        if (!(nx == end.x && ny == end.y))
        {
          cv::Rect roi(nx * occ_size, ny * occ_size, occ_size, occ_size);
          image(roi) = cv::Vec3b(0, 255, 0); // Green color
          cv::imshow("BFS Expansion", image);
          cv::waitKey(1);
        }
      }
    }
  }

  // No path found
  return {};
}

// 启发函数 - 曼哈顿距离
int heuristic(const Point &a, const Point &b)
{
  return std::abs(a.x - b.x) + std::abs(a.y - b.y);
}

// A* 搜索算法
std::vector<Point> aStarSearch(std::vector<std::vector<int>> &grid, Point start, Point end, cv::Mat &image)
{
  int rows = grid.size();
  int cols = grid[0].size();
  std::vector<std::vector<bool>> visited(rows, std::vector<bool>(cols, false));
  std::vector<std::vector<Point>> parent(rows, std::vector<Point>(cols, {-1, -1}));
  std::vector<std::vector<int>> g_cost(rows, std::vector<int>(cols, INT_MAX));

  auto cmp = [](const std::pair<Point, int> &left, const std::pair<Point, int> &right)
  { return left.second > right.second; };
  std::priority_queue<std::pair<Point, int>, std::vector<std::pair<Point, int>>, decltype(cmp)> pq(cmp);
  pq.push({start, 0});
  g_cost[start.y][start.x] = 0;

  // 定义节点可以拓展的方向：以节点为中心，上下左右，四连通
  int dx[] = {-1, 0, 1, 0, -1, 1, -1, 1};
  int dy[] = {0, 1, 0, -1, -1, -1, 1, 1};

  while (!pq.empty())
  {
    Point current = pq.top().first;
    pq.pop();

    // 判断是否到达终点
    if (current.x == end.x && current.y == end.y)
    {
      // 回溯路径
      std::vector<Point> path;
      while (current.x != -1 && current.y != -1)
      {
        path.emplace_back(current);
        current = parent[current.y][current.x];
      }
      std::reverse(path.begin(), path.end());
      return path;
    }

    visited[current.y][current.x] = true;

    // 节点拓展过程
    for (int i = 0; i < 8; ++i)
    {
      int nx = current.x + dx[i];
      int ny = current.y + dy[i];

      if (nx >= 0 && nx < cols && ny >= 0 && ny < rows && !visited[ny][nx] && grid[ny][nx] == 0)
      {
        int new_g_cost = g_cost[current.y][current.x] + 1;
        int h_cost = heuristic({nx, ny}, end);
        int f_cost = new_g_cost + h_cost;

        if (new_g_cost < g_cost[ny][nx])
        {
          g_cost[ny][nx] = new_g_cost;
          pq.push({{nx, ny}, f_cost});
          parent[ny][nx] = current;

          // 显示该节点
          if (!(nx == end.x && ny == end.y))
          {
            cv::Rect roi(nx * occ_size, ny * occ_size, occ_size, occ_size);
            image(roi) = cv::Vec3b(0, 255, 0); // 绿色
            cv::imshow("A* Expansion", image);
            cv::waitKey(1);
          }
        }
      }
    }
  }

  // 未找到路径
  return {};
}

// 辅助函数——计算夹角
double angleBetween(const Point &a, const Point &b, const Point &c)
{
  // 向量 AB 和 AC
  double ab_x = b.x - a.x;
  double ab_y = b.y - a.y;
  double ac_x = c.x - a.x;
  double ac_y = c.y - a.y;

  // 计算角度
  double dot_product = ab_x * ac_x + ab_y * ac_y;
  double magnitude_ab = std::sqrt(ab_x * ab_x + ab_y * ab_y);
  double magnitude_ac = std::sqrt(ac_x * ac_x + ac_y * ac_y);

  double cos_theta = dot_product / (magnitude_ab * magnitude_ac);
  return std::acos(cos_theta) * (180.0 / CV_PI);
}

// AP Theta* 搜索算法
std::vector<Point> apThetaSearch(std::vector<std::vector<int>> &grid, Point start, Point end, cv::Mat &image)
{
  int rows = grid.size();
  int cols = grid[0].size();
  std::vector<std::vector<bool>> visited(rows, std::vector<bool>(cols, false));
  std::vector<std::vector<Point>> parent(rows, std::vector<Point>(cols, {-1, -1}));
  std::vector<std::vector<int>> g_cost(rows, std::vector<int>(cols, std::numeric_limits<int>::max()));
  std::vector<std::vector<double>> lb(rows, std::vector<double>(cols, -std::numeric_limits<double>::infinity()));
  std::vector<std::vector<double>> ub(rows, std::vector<double>(cols, std::numeric_limits<double>::infinity()));

  auto cmp = [](const std::pair<Point, double> &left, const std::pair<Point, double> &right)
  { return left.second > right.second; };
  std::priority_queue<std::pair<Point, double>, std::vector<std::pair<Point, double>>, decltype(cmp)> pq(cmp);
  pq.push({start, 0});
  g_cost[start.y][start.x] = 0;

  // 定义节点可以拓展的方向：以节点为中心，上下左右和对角线，八连通
  int dx[] = {-1, 0, 1, 0, -1, 1, -1, 1};
  int dy[] = {0, 1, 0, -1, -1, -1, 1, 1};

  while (!pq.empty())
  {
    Point current = pq.top().first;
    pq.pop();

    if (visited[current.y][current.x])
      continue;
    visited[current.y][current.x] = true;

    if (current == end)
    {
      // 回溯路径
      std::vector<Point> path;
      while (current.x != -1 && current.y != -1)
      {
        path.emplace_back(current);
        current = parent[current.y][current.x];
      }
      std::reverse(path.begin(), path.end());
      return path;
    }

    for (int i = 0; i < 8; ++i)
    {
      int nx = current.x + dx[i];
      int ny = current.y + dy[i];

      if (nx >= 0 && nx < cols && ny >= 0 && ny < rows && grid[ny][nx] == 0)
      {
        double new_g_cost = g_cost[current.y][current.x] + 1;
        double h_cost = heuristic({nx, ny}, end);
        double f_cost = new_g_cost + h_cost;

        if (new_g_cost < g_cost[ny][nx])
        {
          double angle = angleBetween(parent[current.y][current.x], current, {nx, ny});
          if (angle >= lb[current.y][current.x] && angle <= ub[current.y][current.x])
          {
            g_cost[ny][nx] = new_g_cost;
            pq.push({{nx, ny}, f_cost});
            parent[ny][nx] = current;

            // 显示该节点
            if (!(nx == end.x && ny == end.y))
            {
              cv::Rect roi(nx * occ_size, ny * occ_size, occ_size, occ_size);
              image(roi) = cv::Vec3b(0, 255, 0); // 绿色
              cv::imshow("AP Theta* Expansion", image);
              cv::waitKey(1);
            }
          }
        }
      }
    }
  }

  // 未找到路径
  return {};
}

/*无障碍物*/
std::string pic = "/home/icebot/Nav2_demo/Image/6.png"; // 设置地图
Point start = {4 * occ_size, 5 * occ_size};             // 设置起点
Point end = {7 * occ_size, 5 * occ_size};               // 设置终点
// Point start = {40, 50}; // 设置起点
// Point end = {70, 50};   // 设置终点

// /*有障碍物*/
// std::string pic="1.png";   //设置地图
// Point start = {30, 50};    // 设置起点
// Point end = {88, 50};      //设置终点

int main()
{
  // 读取图片
  cv::Mat recive_im = cv::imread(pic);

  if (recive_im.empty())
  {
    std::cerr << "无法读取图像文件" << std::endl;
    return -1;
  }

  // 缩放图片到所需的尺寸（栅格地图的大小）
  cv::Size new_size(occ_size * 10, occ_size * 10); // 根据需要调整大小
  cv::resize(recive_im, recive_im, new_size, 0, 0, cv::INTER_LINEAR);

  // 创建栅格地图，0表示可通行，1表示障碍
  std::vector<std::vector<int>> grid_map;

  for (int y = 0; y < new_size.height; ++y)
  {
    std::vector<int> row;
    for (int x = 0; x < new_size.width; ++x)
    {
      cv::Vec3b pixel = recive_im.at<cv::Vec3b>(y, x);
      if (pixel == cv::Vec3b(0, 0, 0))
      { // 假设黑色为障碍
        row.emplace_back(1);
      }
      else
      {
        row.emplace_back(0);
      }
    }
    grid_map.emplace_back(row);
  }

  // 打印栅格地图
  for (const auto &row : grid_map)
  {
    for (int value : row)
    {
      std::cout << value << " ";
    }
    std::cout << std::endl;
  }

  // 坐标系定义：
  //  Ori*---------->x
  //     |
  //     |
  //     |
  //     |
  //     v
  //     y

  // // 设置起、终点
  // Point start = {20, 50};
  // Point end = {88, 50};

  // 显示地图
  cv::Mat output_image(grid_map.size() * occ_size, grid_map[0].size() * occ_size, CV_8UC3, cv::Scalar(255, 255, 255));
  markStart(output_image, start);
  markGoal(output_image, end);
  markObstacles(output_image, grid_map); // 标记障碍物

  // 搜索路径
  // std::vector<Point> path = bfsSearch(grid_map, start, end, output_image);
  // std::vector<Point> path = aStarSearch(grid_map, start, end, output_image);
  std::vector<Point> path = apThetaSearch(grid_map, start, end, output_image);

  // cv::imshow("BFS Expansion", output_image);
  // cv::imshow("A* Expansion", output_image);
  cv::imshow("AP Theta* Expansion", output_image);
  cv::waitKey(0); // Wait for user input

  // 打印、显示路径
  if (path.empty())
  {
    std::cout << "未找到路径" << std::endl;
  }
  else
  {
    std::cout << "找到路径，路径长度: " << path.size() << std::endl;
    // 显示路径，但是不显示起点和终点
    for (size_t i = 1; i < path.size() - 1; ++i)
    {
      std::cout << "(" << path[i].x << ", " << path[i].y << ") -> ";
      // 标记路径为红色
      cv::Rect roi(path[i].x * occ_size, path[i].y * occ_size, occ_size, occ_size);
      output_image(roi) = cv::Vec3b(0, 0, 255); // 红色
    }
    std::cout << std::endl;
  }

  // cv::imshow("BFS Expansion with Path", output_image);
  // cv::imshow("A* Expansion with Path", output_image);
  cv::imshow("AP Theta* Expansion with Path", output_image);
  cv::waitKey(0); // Wait for user input

  return 0;
}
